#include <BootConsole.h>
#include <Utils/Bitmap.h>
#include <Utils/Math.h>
#include <Utils/String.h>
#include <Types.h>

#define BitsPerEntry	(sizeof(uint64_t) * 8)

namespace Utils
{

	Bitmap::Bitmap()
	{
	}

	Bitmap::Bitmap(uint64_t count)
	{
		Init((uintptr_t) (new uint64_t[count / BitsPerEntry]), count);
	}

	Bitmap::Bitmap(uintptr_t pos, uint64_t count)
	{
		Init(pos, count);
	}

	void Bitmap::Init(uintptr_t pos, uint64_t count, bool reset)
	{
		if (pos == 0)
			throw ArgumentNullException("Can't put the bitmap at 0x00000000");

		if (count <= 0)
			throw ArgumentOutOfRangeException("The size of bitmap can't be <= to 0");

		Data = (uint64_t *) pos;
		Size = count;
		SizeInEntries = Math::CeilInt((uint64_t) Size, BitsPerEntry) / BitsPerEntry;

		if (reset)
			Reset();
	}

	void Bitmap::Reset()
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		for (uint64_t i = 0; i < SizeInEntries; i++)
			Data[i] = 0;
	}

	void Bitmap::Set(uint64_t index)
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		if (index > Size)
			throw ArgumentOutOfRangeException("Index out of Bitmap");

		int unit = index / BitsPerEntry;
		int offset = index % BitsPerEntry;
		Data[unit] |= (1 << offset);
	}

	void Bitmap::Clear(uint64_t index)
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		if (index > Size)
			throw ArgumentOutOfRangeException("Index out of Bitmap");

		int unit = index / BitsPerEntry;
		int offset = index % BitsPerEntry;
		Data[unit] &= ~(1 << offset);
	}

	bool Bitmap::Test(uint64_t index) const
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		if (index > Size)
			throw ArgumentOutOfRangeException("Index out of Bitmap");

		int unit = index / BitsPerEntry;
		int offset = index % BitsPerEntry;
		return Data[unit] & (1 << offset);
	}

	void Bitmap::Massive(uintptr_t base, uint64_t count, bool value)
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		if (count < 0)
			throw ArgumentOutOfRangeException("Count must be non-negative");
		if (count == 0)
			return;

		if (base + count > Size)
			throw ArgumentOutOfRangeException("Base + Count out of bitmap boundaries");

		if (!MassiveStage1(base, count))
			return;

		if (!MassiveStage2(base, count))
			return;

		MassiveStage3(base, count);
	}

	bool Bitmap::MassiveStage1(uintptr_t &base, uint64_t &pages)
	{
		/* Round base to match the beginning of an uintptr_t */
		while (base % (BitsPerEntry) && pages)
		{
			Set(base++);
			pages--;
		}

		if (!pages)
			return false;

		return true;
	}

	bool Bitmap::MassiveStage2(uintptr_t &base, uint64_t &pages)
	{
		/* Set pages in blocks of PagesPerEntry pages. Much faster than a normal loop */
		while (pages / BitsPerEntry)
		{
			int index = (base) / BitsPerEntry;
			Data[index] = -1;
			base += BitsPerEntry;
			pages -= BitsPerEntry;
		}

		if (!pages)
			return false;

		return true;
	}

	void Bitmap::MassiveStage3(uint64_t base, uint64_t pages)
	{
		while (pages)
		{
			Set(base++);
			pages--;
		}
	}

	uint64_t Bitmap::FindFirst(uint64_t base, bool Value) const
	{
		if (Data == 0)
			throw ArgumentNullException("This bitmap has not been initialized yet");

		int index = GetFirstIndex((uint64_t) base, Value);
		int offset = GetFirstOffset(index, base % BitsPerEntry, Value);

		if (Test(index * BitsPerEntry + offset) != Value)
			throw WtfException("Just got offset in bitmap in FindFirst... but's not good. TODO: USE LOCKS!!!");

		return (index * BitsPerEntry + offset);
	}

	uint64_t Bitmap::GetFirstIndex(uint64_t FirstEntry, bool Value) const
	{
		if (FirstEntry < 0)
			throw ArgumentOutOfRangeException("GetFirstIndex: FirstEntry < 0");

		uint64_t start = FirstEntry / BitsPerEntry;
		for (; start < SizeInEntries; start++)
		{
			if (IndexContains(start, Value))
				return start;
		}
		
		throw OutOfMemoryException();
	}

	uint64_t Bitmap::GetFirstOffset(uint64_t index, uint64_t startOffset, bool Value) const
	{
		if (index < 0 || startOffset < 0 || startOffset > BitsPerEntry)
			throw ArgumentOutOfRangeException(String("GetFirstOffset: index is ") + index + ". startOffset is " + startOffset);

		for (; startOffset < BitsPerEntry; startOffset++)
		{
			if (((Data[index] >> startOffset) & 1) == Value)
				return startOffset;
		}

		throw WtfException("There should be an usable bit in this index. But there isn't -.-");
	}

	bool Bitmap::IndexContains(uint64_t index, bool Value) const
	{
		return Data[index] != (Value ? 0 : -1);
	}
}
